#include "Kalman.h" 

kalmanfilter kf;

// Initialize the Kalman filter
void KalmanInit(kalmanfilter* kf,float initial_P,float Q, float R, float initial_x) 
{
    kf->Output = initial_x;
    kf->P = initial_P;
    kf->Q = Q;
    kf->R = R;
}
 
// Update the Kalman filter with a new measurement
float KalmanFilter(kalmanfilter *kf, float input)
{
    //估算协方差方程：当前 估算协方差 = 上次更新 协方差 + 过程噪声协方差
    kf->P = kf->P + kf->Q;
 
    //卡尔曼增益方程：当前 卡尔曼增益 = 当前 估算协方差 / （当前 估算协方差 + 测量噪声协方差）
    kf->G = kf->P / (kf->P + kf->R);
 
    //更新最优值方程：当前 最优值 = 当前 估算值 + 卡尔曼增益 * （当前 测量值 - 当前 估算值）
    kf->Output = kf->Output + kf->G * (input - kf->Output); //当前 估算值 = 上次 最优值
 
    //更新 协方差 = （1 - 卡尔曼增益） * 当前 估算协方差。
    kf->P = (1 - kf->G) * kf->P;
 
     return kf->Output;
}


